#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
#include <geometry_msgs/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <upros_message/MultipleServo.h>
#include <upros_message/SingleServo.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Bool.h>

using namespace std;

bool received_start_command = false;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

void startCommandCallback(const std_msgs::Bool::ConstPtr& msg)
{
    if(msg->data)
    {
        ROS_INFO("接收到启动指令，开始执行主程序...");
        received_start_command = true;
    }
}

void sleep(double second)
{
    ros::Duration(second).sleep();
}

bool rotateToAngle(ros::NodeHandle& nh, double target_angle, 
                  ros::Publisher& pub, MoveBaseClient& ac) {
    // 获取当前机器人朝向
    tf::TransformListener listener;
    tf::StampedTransform transform;
    
    try {
        listener.waitForTransform("map", "base_link", 
                                 ros::Time(0), ros::Duration(10.0));
        listener.lookupTransform("map", "base_link", 
                                ros::Time(0), transform);
    } catch (tf::TransformException &ex) {
        ROS_ERROR("%s", ex.what());
        return false;
    }
    



     
    // 创建MoveBase目标
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    
    // 设置目标位置为当前位置，仅改变朝向
    goal.target_pose.pose.position.x = transform.getOrigin().x();
    goal.target_pose.pose.position.y = transform.getOrigin().y();
    
    tf2::Quaternion q;
    q.setRPY(0, 0, target_angle);
    goal.target_pose.pose.orientation.x = q.x();
    goal.target_pose.pose.orientation.y = q.y();
    goal.target_pose.pose.orientation.z = q.z();
    goal.target_pose.pose.orientation.w = q.w();
    
    // 发送目标
    ac.sendGoal(goal);
    
    // 等待结果
    bool finished_within_time = ac.waitForResult(ros::Duration(30.0));
    
    if (!finished_within_time) {
        ac.cancelGoal();
        ROS_WARN("Timed out achieving rotation goal");
        return false;
    }
    
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
        ROS_INFO("Rotation to %.2f rad completed successfully", target_angle);
        return true;
    } else {
        ROS_WARN("Rotation failed for some reason");
        return false;
    }
}
bool rotateToTargetAngle(ros::NodeHandle& nh, ros::Publisher& pub, double target_angle, double max_speed, double min_speed, double tolerance = 0.0224) {
    tf::TransformListener listener;
    ros::Rate rate(60); // 控制频率30Hz
    
    while (ros::ok()) {
        // 获取当前角度
        double current_yaw = 0.0;
        try {
            tf::StampedTransform transform;
            listener.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(0.5));
            listener.lookupTransform("map", "base_link", ros::Time(0), transform);
            
            tf::Quaternion q = transform.getRotation();
            tf::Matrix3x3 m(q);
            double roll, pitch, yaw;
            m.getRPY(roll, pitch, yaw);
            current_yaw = yaw;
        } catch (tf::TransformException &ex) {
            ROS_WARN("%s", ex.what());
            continue;
        }

        // 计算角度误差（处理角度环绕）
        double error = target_angle - current_yaw;
        if (error > M_PI) error -= 2*M_PI;
        if (error < -M_PI) error += 2*M_PI;

        ROS_INFO("Rotating - Current: %.2f°, Target: %.2f°, Error: %.2f°", 
                current_yaw*180/M_PI, target_angle*180/M_PI, error*180/M_PI);

        // 检查是否达到目标角度（容忍±3°误差）
        if (fabs(error) <= tolerance) {
            ROS_INFO("Rotation completed within tolerance (%.1f°)", tolerance*180/M_PI);
            // 发布停止命令
            geometry_msgs::Twist stop_cmd;
            pub.publish(stop_cmd);
            return true;
        }

        // Bang-Bang控制
        geometry_msgs::Twist vel_cmd;
        vel_cmd.angular.z = (error > 0) ? 
            ((fabs(error) > 0.5) ? max_speed : min_speed) : 
            ((fabs(error) > 0.5) ? -max_speed : -min_speed);
        
        pub.publish(vel_cmd);
        rate.sleep();
    }
    return false;
}

int main(int argc, char **argv)
{
   
    ros::init(argc, argv, "send_goals_node");
    ros::NodeHandle nh;
    ros::NodeHandle n("~");
    ros::Subscriber start_sub = nh.subscribe("/start_command", 1, startCommandCallback);

    ROS_INFO("等待接收启动指令...");
    ros::Rate wait_rate(10); // 10Hz
    while(ros::ok() && !received_start_command)
    {
        ros::spinOnce();
        wait_rate.sleep();
    }

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Publisher multiple_joint_pub_ = nh.advertise<upros_message::MultipleServo>("/multiple_servo_topic", 10);
    ros::Publisher single_joint_pub_ = nh.advertise<upros_message::SingleServo>("/single_servo_topic", 10);
    MoveBaseClient ac("move_base", true);



// 定义舵机控制函数
    auto moveServo = [&]() {
        upros_message::MultipleServo multiple_servo3;
        for (int i = 1; i < 6; i++) {
            upros_message::SingleServo single_servo3;
            single_servo3.ID = i;
            single_servo3.Rotation_Speed = 160;
            if (i == 1) single_servo3.Target_position_Angle = 850;
            if (i == 2) single_servo3.Target_position_Angle = -700;
            if (i == 3) single_servo3.Target_position_Angle = 0;
            if (i == 4) single_servo3.Target_position_Angle = 0;
            if (i == 5) single_servo3.Target_position_Angle = -20;
            multiple_servo3.servo_gather.push_back(single_servo3);
        }
        multiple_joint_pub_.publish(multiple_servo3);
        ROS_INFO("Servo control commands sent");
    };




    ac.waitForServer();

    double grab_1_x = 2.21;  // movebase目标点x，第一张桌子前方的导航点
    double grab_1_y = 2.24; // movebase目标点y，第一张桌子前方的导航点
    
    double grab_2_x = 2.20;  // movebase目标点x，第二张桌子前方的导航点
    double grab_2_y = 0.25; // movebase目标点y，第二张桌子前方的导航点

    double offset_left = 0.4;  // 左侧偏移量，单位为米
    double offset_right = 0.4; // 右侧偏移量，单位为米

    // 使用参数服务器获取参数，若不需要参数服务器，可将以下代码注释掉
    /*------------------------------------------------------------------------*/
     nh.getParam("/complete_flow_node/grab_1_x", grab_1_x);
     nh.getParam("/complete_flow_node/grab_1_y", grab_1_y);
     nh.getParam("/complete_flow_node/grab_2_x", grab_2_x);
     nh.getParam("/complete_flow_node/grab_2_y", grab_2_y);
     nh.getParam("/complete_flow_node/offset_left", offset_left);
     nh.getParam("/complete_flow_node/offset_right", offset_right);
    /*------------------------------------------------------------------------*/
        geometry_msgs::Twist vel_msg;
         double target_angle_rad = 0;  // 0度
        // rotateToAngle(nh, target_angle_rad, pub, ac) ;
// rotateToTargetAngle(nh, pub, -M_PI/2+0.14, 1.5, 0.7);
 int count = 0;

        upros_message::MultipleServo multiple_servo5;
    for (int i = 1; i < 7; i++)
    {
        upros_message::SingleServo single_servo3;
        single_servo3.ID = i ;
        single_servo3.Rotation_Speed = 30;
        if(i == 1){
            single_servo3.Target_position_Angle = -30;
        }
        if(i == 2){
            single_servo3.Target_position_Angle = -370;
        }
        if(i == 3){
            single_servo3.Target_position_Angle = -350;
        }
        if(i == 4){
            single_servo3.Target_position_Angle = -400;
        }
        if(i == 5){
            single_servo3.Target_position_Angle = 50;
        }
        if(i == 6){
            single_servo3.Target_position_Angle = 750;
        }
        multiple_servo5.servo_gather.push_back(single_servo3);
    }
        multiple_joint_pub_.publish(multiple_servo5);


    
    move_base_msgs::MoveBaseGoal goal;

    // 发送抓取导航点,桌子前方一小段距离
    
    goal.target_pose.pose.position.x = grab_1_x;
    goal.target_pose.pose.position.y = grab_1_y;
    goal.target_pose.pose.orientation.z = 0.0;//0.707
    goal.target_pose.pose.orientation.w = 1.0;//0.707
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        vel_msg.angular.z = 0.0;
        vel_msg.linear.x = 0.0;
        pub.publish(vel_msg);
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
 
        system("roslaunch clean_desktop_robot arm_grab.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }
    // sleep(0.3);
    // vel_msg.angular.z = -0.9;//0,9
    // count = 0;
    // while (ros::ok() && count < 10)//13
    // {
    //     pub.publish(vel_msg);
    //     ros::spinOnce();
    //     loop_rate.sleep();
    //     count++;
    // }
    // vel_msg.angular.z = 0.0;

ros::Rate loop_rate(10);
    vel_msg.angular.z = 1.4;
            count = 0;
           while (ros::ok() && count < 5) //7
            {
                pub.publish(vel_msg);
                ros::spinOnce();
                loop_rate.sleep();
                count++;
                    }
            vel_msg.angular.z = 0.0;
            pub.publish(vel_msg);



   
    upros_message::MultipleServo multiple_servo;
    for (int i = 1; i < 6; i++)
    {
        upros_message::SingleServo single_servo;
        single_servo.ID = i ;
        single_servo.Rotation_Speed = 80;
        if(i == 1){
            single_servo.Target_position_Angle = -900;
        }
        if(i == 2){
            single_servo.Target_position_Angle = -700;
        }
        if(i == 3){
            single_servo.Target_position_Angle = 0;
        }
        if(i == 4){
            single_servo.Target_position_Angle = 0;
        }
        if(i == 5){    
            single_servo.Target_position_Angle = 50;
        }
        multiple_servo.servo_gather.push_back(single_servo);
    }
        multiple_joint_pub_.publish(multiple_servo);



    // // 发送放置导航点
    goal.target_pose.pose.position.x = grab_1_x;//-0.01
    goal.target_pose.pose.position.y = grab_1_y - offset_right; // 放置点位于抓取点右侧约0.4米
    goal.target_pose.pose.orientation.z = 0.707107;//0.707107
    goal.target_pose.pose.orientation.w = 0.707107;
    goal.target_pose.header.frame_id = "map";
     count = 0;
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        // 先前进一小段,大约18cm

    }
    
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }
    //sleep(0.5);  
    upros_message::SingleServo single_servo9;
    single_servo9.ID = 6;
    single_servo9.Rotation_Speed = 160;
    single_servo9.Target_position_Angle = 750;
    single_joint_pub_.publish(single_servo9);
    sleep(0.7);  
       /*vel_msg.angular.z = 1.2;
            count = 0;
           while (ros::ok() && count < 11) //7
            {
                pub.publish(vel_msg);
                ros::spinOnce();
                loop_rate.sleep();
                count++;
                    }
            vel_msg.angular.z = 0.0;
            pub.publish(vel_msg); */   
    upros_message::MultipleServo multiple_servo8;
    for (int i = 1; i < 7; i++)
    {
        upros_message::SingleServo single_servo6;
        single_servo6.ID = i ;
        single_servo6.Rotation_Speed = 100;
        if(i == 1){
            single_servo6.Target_position_Angle = -30;
        }
        if(i == 2){
            single_servo6.Target_position_Angle = -370;
        }
        if(i == 3){
            single_servo6.Target_position_Angle = -350;
        }
        if(i == 4){
            single_servo6.Target_position_Angle = -400;
        }
        if(i == 5){
            single_servo6.Target_position_Angle = 50;
        }
        if(i == 6){
            single_servo6.Target_position_Angle = 750;
        }
        multiple_servo8.servo_gather.push_back(single_servo6);
    }
        multiple_joint_pub_.publish(multiple_servo8);

    




    goal.target_pose.pose.position.x = grab_2_x;
    goal.target_pose.pose.position.y = grab_2_y;
    goal.target_pose.pose.orientation.z = 0.0;
    goal.target_pose.pose.orientation.w = 1.0;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");


 


    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        vel_msg.linear.x = 0.0;
        pub.publish(vel_msg);
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        //rotateToTargetAngle(nh, pub, 0, 1.2, 0.3);

        // 先前进一小段,大约10cm
        geometry_msgs::Twist vel_msg;
        
        // 抓取物块
        system("roslaunch clean_desktop_robot arm_grab.launch");
            // vel_msg.linear.x = -0.4;
    vel_msg.angular.z = -1.4;
            count = 0;
           while (ros::ok() && count < 5) //7
            {
                pub.publish(vel_msg);
                ros::spinOnce();
                loop_rate.sleep();
                count++;
                    }
            vel_msg.angular.z = 0.0;
            pub.publish(vel_msg);
            // vel_msg.linear.x = 0.0;

        upros_message::MultipleServo multiple_servo10;
    for (int i = 1; i < 6; i++)
    {
        upros_message::SingleServo single_servo;
        single_servo.ID = i ;
        single_servo.Rotation_Speed = 120;
        if(i == 1){
            single_servo.Target_position_Angle = 900;
        }
        if(i == 2){
            single_servo.Target_position_Angle = -700;
        }
        if(i == 3){
            single_servo.Target_position_Angle = 0;
        }
        if(i == 4){
            single_servo.Target_position_Angle = 0;
        }
        if(i == 5){    
            single_servo.Target_position_Angle = 50;
        }
        multiple_servo10.servo_gather.push_back(single_servo);
    }
        multiple_joint_pub_.publish(multiple_servo10);
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    // vel_msg.angular.z = 0.9;
    // count = 0;
    // while (ros::ok() && count < 13)
    // {
    //     pub.publish(vel_msg);
    //     ros::spinOnce();
    //     loop_rate.sleep();
    //     count++;
    // }
    // rotateToTargetAngle(nh, pub, M_PI/2-0.14, 1.0, 0.5);
    // sleep(0.1);

 goal.target_pose.pose.position.x = grab_2_x+0.02;
    goal.target_pose.pose.position.y = grab_2_y + offset_left; // 放置点位于抓取点左侧约0.4米
    goal.target_pose.pose.orientation.z = -0.707107;
    goal.target_pose.pose.orientation.w = 0.707107;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
   
    // vel_msg.angular.z = 0.0;
    // target_angle_rad = M_PI/2;
    // rotateToAngle(nh, target_angle_rad, pub, ac) ;
    // pub.publish(vel_msg);
    // vel_msg.linear.x = 0.3;
    // count = 0;
    // while (ros::ok() && count <19)
    // {
    //     pub.publish(vel_msg);
    //     ros::spinOnce();
    //     loop_rate.sleep();
    //     count++;
    // }
    //  vel_msg.linear.x = 0.0;
    // pub.publish(vel_msg);

    upros_message::SingleServo single_servo1;
    single_servo1.ID = 6;
    single_servo1.Rotation_Speed = 160;
    single_servo1.Target_position_Angle = 750;
    single_joint_pub_.publish(single_servo1);
    sleep(0.7);
     vel_msg.angular.z = 1.6;
             count = 0;
            while (ros::ok() && count < 6) //7
             {
                 pub.publish(vel_msg);
                 ros::spinOnce();
                 loop_rate.sleep();
                 count++;
                     }
             vel_msg.angular.z = 0.0;
             pub.publish(vel_msg);
    upros_message::MultipleServo multiple_servo6;
    for (int i = 1; i < 7; i++)
    {
        upros_message::SingleServo single_servo3;
        single_servo3.ID = i ;
        single_servo3.Rotation_Speed = 30;
        if(i == 1){
            single_servo3.Target_position_Angle = 0;
        }
        if(i == 2){
            single_servo3.Target_position_Angle = 0;
        }
        if(i == 3){
            single_servo3.Target_position_Angle = 0;
        }
        if(i == 4){
            single_servo3.Target_position_Angle = 0;
        }
        if(i == 5){
            single_servo3.Target_position_Angle = 0;
        }
         if(i == 6){
            single_servo3.Target_position_Angle = 600;
        }
        multiple_servo6.servo_gather.push_back(single_servo3);
    }
        multiple_joint_pub_.publish(multiple_servo6);
    

   

    goal.target_pose.pose.position.x = 0.1;
    goal.target_pose.pose.position.y = 0.1;
    goal.target_pose.pose.orientation.z =0.382683;
    goal.target_pose.pose.orientation.w =0.92388;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("Send Goal Home !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Back !!!!");
    }
    else
    {
        ROS_WARN("The Goal Planning Failed for some reason");
    }


    vel_msg.linear.x = -0.5;
    count = 0;
    while (ros::ok() && count < 12)
    {
        pub.publish(vel_msg);
        ros::spinOnce();
        loop_rate.sleep();
        count++;
    }
    vel_msg.linear.x = 0.0;
    pub.publish(vel_msg);
    // 停下

    
    return 0;
    
 
}
